/*
 * (C) Copyright 2005 Davide Brugali, Marco Torchiano
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
 * 02111-1307  USA
 */

package moro;

/**
 * Title:        The MObile RObot Simulation Environment
 * Description:
 * Copyright:    Copyright (c) 2001
 * Company:      Universit� di Bergamo
 * @author Davide Brugali
 * @version 1.0
 */


import java.awt.Color;
import java.awt.Graphics;
import java.awt.SystemColor;

import javax.swing.JPanel;

@SuppressWarnings("serial")
public class OccupancyMap extends JPanel {
	// JB: The class OccupanceMap is responsible for containing the data that has been discovered by
	// the robot. It is also reponsible for drawing this data. Not a great example of cohesion.
	int cellDim = 10;
	int width = 510;
	int height = 460;
	char grid[][] = new char[width/cellDim][height/cellDim];
	private final static char UNKNOWN = 'n';
	private final static char EMPTY = 'e';
	private final static char OBSTACLE = 'o';
	private final static char UNREACHABLE_UNKNOWN = 'u';
	
	private final static int NORTH 	= 	270,
			EAST 					= 	0,
			SOUTH 					= 	90,
			WEST 					=	180;

	public OccupancyMap() {
		this.setBackground(SystemColor.window);
		for(int i=0; i < width/cellDim; i++)
			for(int j=0; j < height/cellDim; j++)
				grid[i][j] = UNKNOWN;
		
		this.repaint();
	}

	public void paintComponent(Graphics g) {
		super.paintComponent(g);
		// --------------------------- draw map --------------------------------------
		for(int i=0; i < width/cellDim; i++)
			for(int j=0; j < height/cellDim; j++) {
				if(grid[i][j] == UNKNOWN) {
					g.setColor(Color.yellow);
					g.fillRect(i*cellDim, j*cellDim, cellDim, cellDim);
				}
				else if(grid[i][j] == OBSTACLE) {
					g.setColor(Color.blue);
					g.fillRect(i*cellDim, j*cellDim, cellDim, cellDim);
				}
				else if (grid[i][j] == UNREACHABLE_UNKNOWN)
				{
					g.setColor(Color.black);
					g.fillRect(i * cellDim, j * cellDim, cellDim, cellDim);
				}
			}
		// --------------------------- draw grid --------------------------------------
		g.setColor(Color.lightGray);
		for(int i=0; i <= width/cellDim; i++)
			g.drawLine(i*cellDim, 0, i*cellDim, height);
		for(int j=0; j <= height/cellDim; j++)
			g.drawLine(0, j*cellDim, width, j*cellDim);
	}

	public void drawLaserScan(double position[], double measures[]) {
		double rx = Math.round( position[0] + 20.0 * Math.cos(Math.toRadians(position[2])) );
		double ry = Math.round( position[1] + 20.0 * Math.sin(Math.toRadians(position[2])) );
		int   dir = (int) Math.round(position[2]);

		for(int i=0; i<360; i++) {
			int d = i - dir;
			while(d < 0)
				d += 360;
			while(d >= 360)
				d -= 360;
			double fx = Math.round( rx + measures[d] * Math.cos(Math.toRadians(i)) );
			double fy = Math.round( ry + measures[d] * Math.sin(Math.toRadians(i)) );

			if(measures[d] < 100)
				drawLaserBeam(rx, ry, fx, fy, true);
			else
				drawLaserBeam(rx, ry, fx, fy, false);
		}
		checkUnreachableUnknowns();
		this.repaint();
	}

	private void drawLaserBeam(double rx, double ry, double x, double y, boolean obstacle) {
		int rxi = (int) Math.ceil(rx / cellDim);
		int ryj = (int) Math.ceil(ry / cellDim);
		int  xi = (int) Math.ceil(x / cellDim);
		int  yj = (int) Math.ceil(y / cellDim);

		if(xi < 0 || yj < 0 || xi >= width/cellDim || yj >= height/cellDim )
			return;

		if(obstacle)
			grid[xi][yj] = OBSTACLE;
		else if(grid[xi][yj] != OBSTACLE)
			grid[xi][yj] = EMPTY;

		int xmin = Math.min(rxi, xi);
		int xmax = Math.max(rxi, xi);
		int ymin = Math.min(ryj, yj);
		int ymax = Math.max(ryj, yj);

		if(rx == x)
			for(int j=ymin; j<=ymax; j++) {
				if(grid[xmin][j] != OBSTACLE)
					grid[xmin][j] = EMPTY;
			}
		else {
			double m = (y - ry) / (x - rx);
			double q = y - m * x;
			for(int i=xmin; i<=xmax; i++) {
				int h = (int) Math.ceil((m * (i*cellDim) + q) / cellDim);
				if(h >= ymin && h <= ymax) {
					if(grid[i][h] != OBSTACLE)
						grid[i][h] = EMPTY;
//					if(grid[i+1][h] != OBSTACLE)
//					grid[i+1][h] = EMPTY;
				}
			}
		}
	}
	private void checkUnreachableUnknowns()
	{
		//Eerst moeten we het aantal cellen bepalen
		// Het aantal horizontale cellen is gelijk aan de breedte van het veld / de breedte van een cel
		// Het aantal verticale cellen is gelijk aan de hoogte van het veld / de hoogte van een cel
		int xCellSize = width / cellDim;
		int yCellSize = height / cellDim;

		// Alle cellen doorlopen
		//Voor elke rij....
		for (int x = 0; x < xCellSize; x++)
		{
			//Elke kolom....
			for (int y = 0; y < yCellSize; y++)
			{
				//Alle cellen die UNKNOWN zijn op UNREACHABLE_UNKNOWN zetten
				if (grid[x][y] == UNKNOWN) grid[x][y] = UNREACHABLE_UNKNOWN;
				
				// Nou kijken of we er echt niet bij kunnen
				// Dit kan door te kijken of één van de cellen er om heen EMPTY zijn
				// Als dit zo is dan mag deze cel weer veranderd worden naar UNKNOWN
				if (grid[x][y] == UNREACHABLE_UNKNOWN)
				{
					if ((x > 0 && grid[x - 1][y] == EMPTY) || (y > 0 && grid[x][y - 1] == EMPTY) || (x < xCellSize - 1 && grid[x + 1][y] == EMPTY)
							|| (y < yCellSize - 1 && grid[x][y + 1] == EMPTY)) grid[x][y] = UNKNOWN;
				}
			}
		}
	}
	
	public Boolean isFullyDiscovered() {
		for (int x = 0; x < width/cellDim; x++) {
			for(int y = 0; y < height/cellDim; y++) {
				if (grid[x][y] == UNKNOWN)
					return false;
			}
		}
		return true;
	}
	
	public Boolean isAreaFree(int[] topleft, int[] bottomright) {
		for (int xi = topleft[0]; xi < bottomright[0]; xi++) {
			for(int yi = topleft[1]; yi < bottomright[1]; yi++) {
				if (grid[xi][yi] == UNKNOWN || grid[xi][yi] == OBSTACLE)
					return false;
			}
		}
		return true;
	}
	
	
	public synchronized int distanceToWall(int[] starts, int dir) {
		Boolean found = false;
		int x = starts[0], y = starts[1];

		int distance = 0;
		while (!found) {
			
			distance+=cellDim;			
			
			if (dir == NORTH) {
				y = starts[1] - distance;
			}
			else if (dir == SOUTH) {
				y = starts[1] + distance;
			}
			else if  (dir == EAST) {
				x = starts[0] + distance;
			}
			else if (dir == WEST) {
				x = starts[0] - distance;

			} else
				return -1;

			if (x <= 0 || x >= width || y <= 0 || y >= height) {
				//System.out.println(x);
				distance-=cellDim;
				found = true; 
			}
			else{
				
				if(dir == NORTH || dir == SOUTH){
					if (grid[Math.round(x/cellDim)][(int)Math.round(y/cellDim)] == UNKNOWN ||
						grid[Math.round(x/cellDim)][(int)Math.round(y/cellDim)] == OBSTACLE) {
						distance-=cellDim;
						found = true;
						if (grid[(int)Math.round(x/cellDim)][(int)Math.round(y/cellDim)] == UNKNOWN &&  distance >= 90) {
							distance = 90;
						}
						break;
					}
				}
				else if(dir == EAST || dir == WEST){
					if (grid[(int)Math.round(x/cellDim)][Math.round(y/cellDim)] == UNKNOWN ||
						grid[(int)Math.round(x/cellDim)][Math.round(y/cellDim)] == OBSTACLE) {
						distance-=cellDim;
						found = true;
						if (grid[(int)Math.round(x/cellDim)][(int)Math.round(y/cellDim)] == UNKNOWN &&  distance >= 90) {
							distance = 90;
						}
						break;
					}
				}
			}				
		}
		return distance;
	}
	
}

